#include "control.h"
#include "global.h"
#include "main.h"

void MotorStartCheck(void)
{
	unsigned int dd = 0;

	if (MOTORSTATE == STATEERROR)
		return;

	if (SetSpeed > 0 && (GPIO_ReadPin(EN_GPIO_PORT, EN_GPIO_PIN) == GPIO_Pin_SET))
	{

		for (dd = 0; dd < 100; dd++)
			;
		if (GPIO_ReadPin(EN_GPIO_PORT, EN_GPIO_PIN) == GPIO_Pin_SET)
			SampleSpeed();
		if (SetSpeed > 0)
		{

			if (GPIO_ReadPin(DR_GPIO_PORT, DR_GPIO_PIN) == GPIO_Pin_SET) // 方向读取
			{
				for (dd = 0; dd < 500; dd++)
					;
				if (GPIO_ReadPin(DR_GPIO_PORT, DR_GPIO_PIN) == GPIO_Pin_SET)
				{
					Dir = 0;
				}
			}
			else if (GPIO_ReadPin(DR_GPIO_PORT, DR_GPIO_PIN) == GPIO_Pin_RESET)
			{
				for (dd = 0; dd < 500; dd++)
					;
				if (GPIO_ReadPin(DR_GPIO_PORT, DR_GPIO_PIN) == GPIO_Pin_RESET)
				{
					Dir = 1;
				}
			}

			MOTORSTATE = STATESTARTDELAY;
			TargS1 = 0;
			TimeCountSTDelay = 0;
		}
	}
}

void MotorStartDealy(void)
{
	if (MOTORSTATE == STATEERROR)
		return;

	if (SetSpeed == 0 && (GPIO_ReadPin(EN_GPIO_PORT, EN_GPIO_PIN) == GPIO_Pin_SET))
	{ // 速度为0或者启动无效
		MOTORSTATE = STATESTOP;
	}

	if (TimeCountSTDelay >= CanshuTimeDelay) // 启动延时200ms
	{
		TargS1 = 0;
		TimeCountSTDelay = 0;
		MOTORSTATE = STATESTARTPID;
	}
}

void MotorStartPID(void)
{
	if (MOTORSTATE == STATEERROR)
		return;

	if (SetSpeed == 0)
	{
		MOTORSTATE = STATESTOP;
	}

	if (TargS1 == 0)
	{
		TargS1 = MINSPEED; // 200
		if (CanshuSTT < 10)
			CanshuSTT = 10;
		SpeedD = MAXSPEED / (CanshuSTT / 10); // 60
		if (SpeedD < 1)
			SpeedD = 1;
		TargS1 += SpeedD;
		if (TargS1 > SetSpeed)
			TargS1 = SetSpeed;
		MOTOR_STOP0();
		OutPwmValue = 480;

		HALL_MOTOR_START();
		PID_init();
		PIDcompute(TargS1, 0);

		TimeCountStartUP = 0;
		TimeCountPID = 0;
		HALLcountTemp = 0;
		RealS1 = 0;
	}
	else if (TargS1 < SetSpeed && TimeCountStartUP >= 10)
	{
		TimeCountStartUP = 0;
		TargS1 += SpeedD;
		if (TargS1 >= SetSpeed)
		{
			TargS1 = SetSpeed;
		}
	}
	else if (TargS1 >= SetSpeed)
	{
		TargS1 = SetSpeed;
	}

	if (TimeCountPID >= 20) // 计算实时速度并时行PID运算
	{
		TimeCountPID = 0;
		RealS1 = HALLcountTemp * 500 / MPolePairs; // HALLcount*50*60/6/POLEPAIRS;
		PIDcompute(TargS1, RealS1);
	}
	if (RealS1 >= SetSpeed)
	{
		TargS1 = SetSpeed;
		MOTORSTATE = STATERUNPID;
	} // 达到设定速度了，才认为启动成功，进入正常PID控制。
}

void MotorRunPID(void)
{
	if (MOTORSTATE == STATEERROR)
		return;
	if (SetSpeed == 0)
	{
		MOTORSTATE = STATESTOP;
	}

	TargS1 = SetSpeed;
	if (TimeCountPID >= 20) // 计算实时速度并时行PID运算
	{
		TimeCountPID = 0;
		RealS1 = HALLcountTemp * 500 / MPolePairs; // HALLcount*50*60/6/POLEPAIRS;//20*50=1s,1s*60=1min,6表示一圈霍尔计数次数是6。
		PIDcompute(TargS1, RealS1);
	}
}

void MotorStop(void)
{
	if (MOTORSTATE == STATEERROR)
		return;
	Motor_Start_F = 0;
	MOTOR_STOP0();

	MOTORSTATE = STATESTARTCHECK;
}

void MotorError(void)
{
	MOTOR_STOP0();
	Motor_Start_F = 0;
	MOTORSTATE = STATEERROROVER;
}

void MotorErrorOver(void)
{
	unsigned char times = 0;

	LEDOFF;
	TimeCountTemp = 0;
	while (TimeCountTemp < 200)
		;
	while (1) // 指示灯闪烁
	{
		if (times < ErrorCode)
		{
			if (TimeCountTemp < 200)
			{
				LEDON;
			}
			else if (TimeCountTemp <= 400) // LED flashing
			{
				LEDOFF;
			}
			else if (TimeCountTemp > 400)
			{
				if (times < ErrorCode)
				{
					times++;
					TimeCountTemp = 0;
					if (times >= ErrorCode)
						;
					else
						LEDOFF;
				}
			}
		}
		else if (TimeCountTemp < 500)
			;
		else
		{
			times = 0;
			TimeCountTemp = 0;
		}

		//		if(ErrorCode==10)
		//		{ //上电电位器没归零时，需要检测电位器数据，归零则清故障，然后可以启动电机
		//			SampleSpeed();
		//			if(SetSpeed==0)
		//			{
		//				ErrorCode=0;LEDON;MOTORSTATE=STATWAITSTART;  break;
		//			}
		//		}
	}
}

void WaitStart(void)
{
	if (SetSpeed == 0)
		MOTORSTATE = STATESTARTCHECK;
}

void ENCheck(void)
{
	unsigned int dd;

	if (MOTORSTATE == STATEERROR)
		return;

	if (GPIO_ReadPin(EN_GPIO_PORT, EN_GPIO_PIN) == GPIO_Pin_RESET)
	{
		for (dd = 0; dd < 500; dd++)
			;
		if (GPIO_ReadPin(EN_GPIO_PORT, EN_GPIO_PIN) == GPIO_Pin_RESET)
		{
			MOTORSTATE = STATESTOP;
			return;
		}
	}
}
